#ifndef __JPDA_DATA_ASSOCIATION_INCLUDE__
#define __JPDA_DATA_ASSOCIATION_INCLUDE__
#include <functional>
#include "TrackInitializer.h"
#include "../comm_define.h"
#include "../UKF/UKF.h"
#include "FIFO.h"
#include "MultiTargetTrackDefine.h"
#include "Hungarian.h"

#include <vector>

typedef std::function<void(TrackInfo&)> onDeleteTrackAction;

enum algoType {
JPDA,
NNJPDA
};

/*
次优JPDA算法实现，算法参考文档如下
《Suboptimal JPDA for Tracking in the Presence of Clutter and Missed Detections 》
作者: Edson H. Aoki  / Karl H. Kienitz
*/

class JPDADataAssociation {
public:
	JPDADataAssociation(onDeleteTrackAction delAct=NULL);
	virtual ~JPDADataAssociation();
public:
	void setPlatformMove(bool isMove);
	void setMaxTrackVelocity(double value);
	//航迹与量测进行关联
	void associatePlot(const CCVBlockVec_t& plots, //当前周期的量测点集
		double  secsPreCycle, //当前的扫描周期（秒）
		int64_t current_timeSecFromEpoch , //当前周期的精确时间戳（精确到毫秒）
		std::vector<size_t>& assignedStatus //量测的关联状态
	);
	//增加新的航迹
	void addNewTentativeTrack(TentativeTrack& trace);
	//清理无效航迹
	void cleanTrack();
	//获取新的未使用的航迹ID
	unsigned int getNewID()  { return globalID++; }
	//判断目标是否满足清批条件
	bool isToDelete(TrackInfo& track);
	//航迹清批时的毁掉函数
	void onDeleteTrack(TrackInfo& track);
	//获取当前所有航迹的列表
	std::vector<TrackInfo>& getCurrentTracks()  { return tracks;  }
	//检测目标是否处于机动状态并采用相应的策略
	//void detectTargetManeuver(TrackInfo& track);
private:
	double likelihood_tj(size_t t, size_t j,
		const std::vector<DistributionParam_t>& pred_z_dist,
		const CCVBlockVec_t& plots);
	double N_tj(size_t t, size_t j,
		const std::vector<DistributionParam_t>& pred_z_dist,
		const CCVBlockVec_t& plots,
		std::vector<std::set<size_t> >& A);

	double N_tj_(size_t t, size_t j,
		const std::vector<DistributionParam_t>& pred_z_dist,
		const CCVBlockVec_t& plots,
		std::vector<std::set<size_t> >& A);

	double D_tj(size_t t, size_t j,
		const std::vector<DistributionParam_t>& pred_z_dist,
		const CCVBlockVec_t& plots,
		std::vector<std::set<size_t> >& A,
		std::vector<std::set<size_t> >& L);

	double CostMinSolve(const MatrixXd& DistMatrix, std::vector<int>& Assignment); 
	double getassociatePlotGate(double R, MovementStatusCode mvoeState); //获取关联门限,用于限制过多的关联，造成计算压力
private:
	std::vector<TrackInfo>		tracks;
	unsigned int				globalID;
	onDeleteTrackAction			deletetrackAct;
	int64_t						current_timeSecFromEpoch;
	MatrixXd					likelihoodDelay;
	MatrixXd					NtjDelay;
	/*
	B是一个偏置项用于控制杂波密度，
	Roecker and Phillis 建议设置为0，除非杂波比较密集。
	备注：在一个移动目标驶过固定目标时，有时固定目标会被移动目标"带跑",
	本程序中设置了一个极小的B参数，令目标有一定的权值保持预估位置不变。
	*/
	double B;
	algoType					algo_type;
	double						max_trackVel_ms;
	bool						isMovePlat;	//是否为移动平台
	HungarianAlgorithm HungAlgo;
};

#endif